Visual SLAM Tutorial: Bundle Adjustment

In a two-view setting, we are interested in finding the most likely camera poses T w 1 and T w 2 , both in the space SE(3) of 3D poses, and 3D points Pw j ∈R3, with j∈ 1..n, which we can do by minimizing the re-projection errors f (T w c ,P w; p) = ‖h(T w c ,Pw)− p‖Σ associated with each measurement p ∈ R2. Above, the measurement function h : SE(3)×R3→ R2 predicts the measurement p from the unknowns T w c = (Rc , tw c ) and Pw, and the notation ‖e‖Σ ∆ = eT Σ−1e is shorthand for the squared Mahalanobis distance with covariance Σ. The objective function is then the log-likelihood of the unknowns given all such measurements p in the two views. Under the Gaussian noise assumptions, this is exactly